/**
* @file        StaticAccChecker
* @brief       
* @version     1.0.0
* @author      Changlin.Jing 
* @date        2023/7/31 15:20
* @copyright   Copyright  2022 Langge Software Co., Ltd. All rights reserved.
*/
#ifndef LANGGE_VDR_STATICSTATEDETECTION_HPP
#define LANGGE_VDR_STATICSTATEDETECTION_HPP

#include <vector>
#include "ut_symbols.h"
#include "loc_types.h"
#include "Eigen/Core"
#include "NaviData.h"

BEGIN_SPACE_LANGE_VDR
class StaticStateDetection {
public:
    StaticStateDetection(int buffLen, double initAccStdThreshold);
    static constexpr double G = 9.7803267715;

    void Reset();
    void Insert(std::shared_ptr<NaviData::ImuData> imuData);
    bool CheckStatic();
    Eigen::Vector2d GetRollPitch();
    bool IsStatic();

private:
    std::vector<Eigen::Vector3d> mVecAcc;
    int mFixedImuSize{};
    int mBuffLength;
    double mAccStdThreshold;
    Eigen::Vector2d mInitRollPitch;


    static void CalVarianceAndMean(std::vector<Eigen::Vector3d>& vec_vec, Eigen::Vector3d& mean, double& variance);

    bool mIsStatic = false;
};

END_SPACE_LANGE_VDR
#endif //LANGGE_VDR_STATICSTATEDETECTION_HPP
